/*!
  @file solver_method.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _SOLVER_METHOD_H_
#define _SOLVER_METHOD_H_

#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "mrover_vo/feature.h"

namespace vo {

/*! 
 *  @brief  Solver method base class
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

template <typename ValType>
class SolverMethod {
 public:
  typedef boost::shared_ptr<SolverMethod<ValType> > Ptr;

  typedef Eigen::Matrix<ValType, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;
  typedef Eigen::Matrix<ValType, 3, 1> Vector3T;
  typedef Eigen::Transform<ValType, 3, Eigen::Affine> Affine3T;

  typedef std::vector<Affine3T,Eigen::aligned_allocator<Affine3T> > Poses;
  typedef boost::shared_ptr<Poses> PosesPtr;
  typedef boost::shared_ptr<Feature::Matches const> MatchesConstPtr;


  SolverMethod() { }

  virtual ~SolverMethod() { }

  inline void compute(const std::vector<int> &samples) { 
    initializeData(samples);
    computePose(); 
  }

  inline void compute(const std::vector<int> &samples, PosesPtr &poses) {
    setOutputPose(poses);
    compute(samples);
  }

  void setInputFeature(const Feature::ConstPtr &feature) { input_ = feature; }
  void setTargetFeature(const Feature::ConstPtr &feature) { target_ = feature; }
  void setInputMatches(const MatchesConstPtr &matches) { matches_ = matches; }
  void setOutputPose(PosesPtr &poses) { poses_ = poses; }

  void setCameraParams(const MatrixXT &_K, const MatrixXT &_lTr) {
    K = _K;
    lTr = _lTr;
  }


  //! returns the number of required samples to compute model parameters
  virtual size_t getRequiredSampleSize() = 0;

  virtual bool checkGoodness(const std::vector<int> &samples, const int new_idx) { return true; }


 protected:
  virtual inline bool initCompute() {
    if (!input_ || !target_ || !matches_ || !poses_) return false;
    return true;
  }

  //! copies data, conversion, whatever
  virtual void initializeData(const std::vector<int> &samples) = 0;

  virtual void computePose() = 0;


  MatrixXT K;
  MatrixXT lTr;

  Feature::ConstPtr input_;
  Feature::ConstPtr target_;
  MatchesConstPtr matches_;
  
  //! Pose matrices which transform points from target to input
  /*! input = iTt * target */
  PosesPtr poses_;
};

} // namespace vo

#endif  // _SOLVER_METHOD_H_
